#include "Ammeter.hpp"
#include "board_base.h"
#include "adc_mcp3421.h"
#include "bsp_uart.h"
#include "iMath.h"


const double Ammeter::VOLTAGE_GAIN = 91.0;
const float Ammeter::SR[4] = {20.0f, 0.1f, 0.02f, 0.004f}; //Sampling Resistors

/**
 * @brief switch sampling resistor
 * @param Gear 
 */
void SamplingResistorSwitch(uint32_t Gear)
{
	switch (Gear)
	{
	case Ammeter::SamplingResistor0d004:
		HAL_GPIO_WritePin(MOS1_GPIO_Port, MOS1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOS2_GPIO_Port, MOS2_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOS3_GPIO_Port, MOS3_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOS4_GPIO_Port, MOS4_Pin, GPIO_PIN_SET);
		break;
	case Ammeter::SamplingResistor0d02:
		HAL_GPIO_WritePin(MOS1_GPIO_Port, MOS1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOS2_GPIO_Port, MOS2_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOS4_GPIO_Port, MOS4_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOS3_GPIO_Port, MOS3_Pin, GPIO_PIN_SET);
		break;
	case Ammeter::SamplingResistor0d1:
		HAL_GPIO_WritePin(MOS1_GPIO_Port, MOS1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOS3_GPIO_Port, MOS3_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOS4_GPIO_Port, MOS4_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOS2_GPIO_Port, MOS2_Pin, GPIO_PIN_SET);
		break;
	case Ammeter::SamplingResistor20:
		HAL_GPIO_WritePin(MOS2_GPIO_Port, MOS2_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOS3_GPIO_Port, MOS3_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOS4_GPIO_Port, MOS4_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOS1_GPIO_Port, MOS1_Pin, GPIO_PIN_SET);
		break;
	default:
		HAL_GPIO_WritePin(MOS1_GPIO_Port, MOS1_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOS2_GPIO_Port, MOS2_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOS3_GPIO_Port, MOS3_Pin, GPIO_PIN_RESET);
		HAL_GPIO_WritePin(MOS4_GPIO_Port, MOS4_Pin, GPIO_PIN_RESET);
		break;
	}
}

Ammeter::Ammeter(void)
{
    this->rate = RATE_240SPS_12BIT;
    this->rvoltage = 0;
    this->current_gear = Ammeter::GEAR_Off;
    kalman1_init(&this->kmfilter_rvoltage, 1, 1, 0.001, 0.1);
}

void Ammeter::setRate(ADCRate Rate)
{
    this->rate = Rate;
    MCP3421_Configure(MCP_AUTO_MODE,
                      Rate,
                      MCP_GAIN_x1);
}

Ammeter::ADCRate Ammeter::getRate(void)
{
    return this->rate;
}
void Ammeter::dataReflash(void)
{
    this->rvoltage = MCP3421_GetVoltage() / VOLTAGE_GAIN;
    this->rvoltage_estimate = kalman1_filter(&this->kmfilter_rvoltage, this->rvoltage);
    if (flag_print == 1)
    {
        Queue_Printf(&htx1, "VA:%.4f,%.4f,%d\n\r", 
            this->rvoltage_estimate*1000, this->getCurrent()*1000, this->getGear());
    }
    if (flag_autoSwitchGear == 1)
    {
        this->setGearAuto();
    }
}

void Ammeter::setGear(GearSelect Gear)
{

    this->current_gear = Gear;
    SamplingResistorSwitch(Gear);
}

/**
 * @brief Get Ammeter Mode
 * @retval uint8_t	(0x08)1000b:0.004R(1-5A)
 * 					(0x04)0100b:0.02R(200mA-1A)
 * 					(0x02)0010b:0.1R(1mA-200mA)
 * 					(0x01)0001b:20R(0~1000uA)
 */
Ammeter::GearSelect Ammeter::getGear(void)
{
    return (GearSelect)this->current_gear;
}

Ammeter::GearSelect Ammeter::setGearAuto(void)
{
    double I = this->getCurrent() * 1000;
    Ammeter::GearSelect gearSet = (Ammeter::GearSelect)this->current_gear;
    for (size_t i = 0; i < 4; i++)
    {
        switch (gearSet)
        {
        case GEAR_1000UA:
            if (I >= 1.0)
            {
                gearSet = GEAR_200MA;
            }
            break;
        case GEAR_200MA:
            if (I >= 200.0)
            {
                gearSet = GEAR_1A;
            }
            else if (I < 1.0)
            {
                gearSet = GEAR_1000UA;
            }
            break;
        case GEAR_1A:
            if (I >= 1000.0)
            {
                gearSet = GEAR_5A;
            }
            else if (I < 190.0)
            {
                gearSet = GEAR_200MA;
            }
            break;
        case GEAR_5A:
            if (I >= 5000.0)
            {
                gearSet = GEAR_Off;
            }
            else if (I < 900.0)
            {
                gearSet = GEAR_1A;
            }
            break;
        default:
            break;
        }
    }
    this->setGear((GearSelect) gearSet);
	return (GearSelect) gearSet;
}


double Ammeter::getVoltage(void)
{
    return this->rvoltage;
}
double Ammeter::getCurrent(void)
{
    double I = this->rvoltage / this->SR[this->current_gear];
	return I;
}

void Ammeter::printStart(void)
{
    this->flag_print = 1;
}

void Ammeter::printStop(void)
{
    this->flag_print = 0;
}

void Ammeter::autoModeStart(void)
{
    this->flag_autoSwitchGear = 1;
}

void Ammeter::autoModeStop(void)
{
    this->flag_autoSwitchGear = 0;
}
